#!/usr/bin/env python
'''
Created on 2013-05-23

@author: Tamer Saadeh
'''

import serial
import time
import rospy

from cStringIO import StringIO

def _OnLineReceived(line):
	print(line)

class SerialData(object):
	'''
	Class for communicating with Arduino via a serial connection.
	'''

	def __init__(self, port="/dev/ttyUSB0", baudrate=115200, lineHandler = _OnLineReceived):
		'''
		Initializes the communicator class. 
		device: The serial device to listen to, defaults to /dev/ttyUSB0.
		baudrate: The baud rate to use when communicating with Arduino, defaults to 115200.
		'''

		self._Device = port
		self._Baudrate = baudrate
		self.ReceivedLineHandler = lineHandler
        	rospy.loginfo("SerialData: Initialized.")


	def Start(self):
		'''
		Opens the communication link with Arduino
		'''

		rospy.loginfo("SerialData: Starting serial connection at: '" + self._Device + "'.")

		try:
		    # timeout after 2 seconds
		    self._Serial = serial.Serial(port = self._Device, baudrate = self._Baudrate, timeout = 2)

		except:
		    rospy.loginfo("SerialData: Failed to open serial connection at: " + self._Device + ", try a different device.")

		else:
		    self._Running = True

		rospy.loginfo("SerialData: Stated serial connection.")


	def Stop(self):
		'''
		Closes the communication link with Arduino.
		'''

		rospy.loginfo("SerialData: Stopping serial connection at: '" + self._Device + "'.")

		try:
			self._Serial.close()

		except:
			rospy.loginfo("SerialData: Nothing to stop.")

		finally:
			self._Running = False

		rospy.loginfo("SerialData: Stopped serial connection.")


	def _Listen(self):
		'''
		Listens to the serial data and forwards it to /serial.
		'''
		stringIO = StringIO()

		rospy.loginfo("SerialData: Listening on serial communication.")

		while self._Running:
			try:
				data = self._Serial.read()

			except:
				rospy.loginfo("SerialData: Failed to read.")
				break
		                        
			else:
				if data == '\r':
                                        pass
                                if data == '\n':
                                        self.ReceivedLineHandler(stringIO.getvalue())
					rospy.loginfo("SerialData: Read a line: '" + stringIO.getvalue() + "'.")
                                        stringIO.close()
                                        stringIO = StringIO()
                                else:
                                        stringIO.write(data)

		rospy.loginfo("SerialData: Finished listening to serial. Connection is being closed.")


	def Write(self, data):
		'''
		Listens to the serial data and forwards it to /serial.
		data: The data that is going to be sent to Arduino.
		'''
		
		rospy.loginfo("SerialData: Writing on serial communication.")

		try:
			rospy.loginfo("SerialData: Trying to write: '" + data + "'.")
			self._Serial.write(data)
		
		except:
			rospy.loginfo("SerialData: Failed to write data to serial.")
